﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;
using starLiGHT.Physic.TwoD;
using Microsoft.Xna.Framework.Graphics;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2011
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Physic2D.

    starLiGHT.Physic2D is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Physic2D is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Physic2D.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Physic2D are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev::                       $:  Revision of last commit *
// * $Author::                    $:  Author of last commit   *
// * $Date::                      $:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Rope/b2Rope.h
// Revision   : r149
// Change-Date: 2011-02-22
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Rope/b2Rope.cpp
// Revision   : r149
// Change-Date: 2011-02-22
//
// Status     : finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public class RopeDef
    {
        public RopeDef()
        {
            this.vertices = null;
            this.count = 0;
            this.masses = null;
            this.gravity = Vector2.Zero;
            this.damping = 0.1f;
            this.k2 = 0.9f;
            this.k3 = 0.1f;
        }

        public Vector2[] vertices;

        public int count;

        public float[] masses;

        public Vector2 gravity;

        public float damping;

        ///<summary>
        ///Stretching stiffness
        ///</summary>
        public float k2;

        /// <summary>
        /// Bending stiffness. Values above 0.5 can make the simulation blow up.
        /// </summary>
        public float k3;
    }

    public class Rope
    {
        public Rope()
        {
            this.m_count = 0;
            this.m_ps = null;
            this.m_p0s = null;
            this.m_vs = null;
            this.m_ims = null;
            this.m_Ls = null;
            this.m_as = null;
            this.m_gravity = Vector2.Zero;
            this.m_k2 = 1.0f;
            this.m_k3 = 0.1f;
        }

        public void Initialize(RopeDef def)
        {
            System.Diagnostics.Debug.Assert(def.count >= 3);
            this.m_count = def.count;
            this.m_ps = new Vector2[this.m_count];
            this.m_p0s = new Vector2[this.m_count];
            this.m_vs = new Vector2[this.m_count];
            this.m_ims = new float[this.m_count];

            for (int i = 0; i < this.m_count; ++i)
            {
                m_ps[i] = def.vertices[i];
                m_p0s[i] = def.vertices[i];
                m_vs[i] = Vector2.Zero;

                float m = def.masses[i];
                if (m > 0f)
                {
                    m_ims[i] = 1.0f / m;
                }
                else
                {
                    m_ims[i] = 0f;
                }
            }

            int count2 = this.m_count - 1;
            int count3 = this.m_count - 2;
            this.m_Ls = new float[count2];
            this.m_as = new float[count3];

            for (int i = 0; i < count2; ++i)
            {
                Vector2 p1 = m_ps[i];
                Vector2 p2 = m_ps[i + 1];
                m_Ls[i] = Vector2.Distance(p1, p2);
            }

            for (int i = 0; i < count3; ++i)
            {
                Vector2 p1 = m_ps[i];
                Vector2 p2 = m_ps[i + 1];
                Vector2 p3 = m_ps[i + 2];

                Vector2 d1 = p2 - p1;
                Vector2 d2 = p3 - p2;

                float a = Vector2Extensions.Cross(d1, d2);
                float b = Vector2.Dot(d1, d2);

                m_as[i] = (float)System.Math.Atan2(a, b);
            }

            this.m_gravity = def.gravity;
            this.m_damping = def.damping;
            this.m_k2 = def.k2;
            this.m_k3 = def.k3;
        }

        public void Step(float timeStep, int iterations)
        {
            if (timeStep == 0f)
            {
                return;
            }

            float d = (float)System.Math.Exp(-timeStep * this.m_damping);

            for (int i = 0; i < this.m_count; ++i)
            {
                m_p0s[i] = m_ps[i];
                if (m_ims[i] > 0f)
                {
                    m_vs[i] += timeStep * this.m_gravity;
                }
                m_vs[i] *= d;
                m_ps[i] += timeStep * m_vs[i];
            }

            for (int i = 0; i < iterations; ++i)
            {
                SolveC2();
                SolveC3();
                SolveC2();
            }

            float inv_h = 1f/timeStep;
            for (int i = 0; i < this.m_count; ++i)
            {
                m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]);
            }
        }

        public int VertexCount
        {
            get
            {
                return this.m_count;
            }
        }

        public Vector2[] Vertices
        {
            get
            {
                return this.m_ps;
            }
        }

        public void Draw(DebugDraw draw)
        {
            Color c = new Color(0.4f, 0.5f, 0.7f, 1.0f);

            for (int i = 0; i < this.m_count - 1; ++i)
            {
                draw.DrawSegment(m_ps[i], m_ps[i + 1], c);
            }
        }

        public void SetAngle(float angle)
        {
            int count3 = m_count - 2;
            for (int i = 0; i < count3; ++i)
            {
                m_as[i] = angle;
            }
        }

        private void SolveC2()
        {
            int count2 = this.m_count - 1;

            for (int i = 0; i < count2; ++i)
            {
                Vector2 p1 = m_ps[i];
                Vector2 p2 = m_ps[i + 1];

                Vector2 d = p2 - p1;
                d.Normalize();
                float L = d.Length();

                float im1 = m_ims[i];
                float im2 = m_ims[i + 1];

                if (im1 + im2 == 0f)
                {
                    continue;
                }

                float s1 = im1 / (im1 + im2);
                float s2 = im2 / (im1 + im2);

                p1 -= m_k2 * s1 * (m_Ls[i] - L) * d;
                p2 += m_k2 * s2 * (m_Ls[i] - L) * d;

                m_ps[i] = p1;
                m_ps[i + 1] = p2;
            }
        }

        private void SolveC3()
        {
            int count3 = m_count - 2;

            for (int i = 0; i < count3; ++i)
            {
                Vector2 p1 = m_ps[i];
                Vector2 p2 = m_ps[i + 1];
                Vector2 p3 = m_ps[i + 2];

                float m1 = m_ims[i];
                float m2 = m_ims[i + 1];
                float m3 = m_ims[i + 2];

                Vector2 d1 = p2 - p1;
                Vector2 d2 = p3 - p2;

                float L1sqr = d1.LengthSquared();
                float L2sqr = d2.LengthSquared();

                if (L1sqr * L2sqr == 0f)
                {
                    continue;
                }

                float a = Vector2Extensions.Cross(d1, d2);
                float b = Vector2.Dot(d1, d2);

                float angle = (float)System.Math.Atan2(a, b);

                Vector2 Jd1 = (-1.0f / L1sqr) * d1.Perpendicular();
                Vector2 Jd2 = (1.0f / L2sqr) * d2.Perpendicular();

                Vector2 J1 = -Jd1;
                Vector2 J2 = Jd1 - Jd2;
                Vector2 J3 = Jd2;

                float mass = m1 * Vector2.Dot(J1, J1) + m2 * Vector2.Dot(J2, J2) + m3 * Vector2.Dot(J3, J3);
                if (mass == 0f)
                {
                    continue;
                }

                mass = 1.0f / mass;

                float C = angle - m_as[i];

                while (C > MathHelper.Pi)
                {
                    angle -= MathHelper.TwoPi;
                    C = angle - m_as[i];
                }

                while (C < -MathHelper.Pi)
                {
                    angle += MathHelper.TwoPi;
                    C = angle - m_as[i];
                }

                float impulse = -m_k3 * mass * C;

                p1 += (m1 * impulse) * J1;
                p2 += (m2 * impulse) * J2;
                p3 += (m3 * impulse) * J3;

                m_ps[i] = p1;
                m_ps[i + 1] = p2;
                m_ps[i + 2] = p3;
            }
        }

        private int m_count;
        private Vector2[] m_ps;
        private Vector2[] m_p0s;
        private Vector2[] m_vs;

        private float[] m_ims;

        private float[] m_Ls;
        private float[] m_as;

        private Vector2 m_gravity;
        private float m_damping;

        private float m_k2;
        private float m_k3;
    }
}
